// --------------------------------------------------------------------------------------------------------------------
// <copyright file="PathTrackingDriveAgent.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Navigation.Runtime.Motion
{
    using System;
    using System.Runtime.Serialization;
    using Microsoft.Robotics.Hardware.DAC;
    using Microsoft.Robotics.Hardware.Runtime.DAC;
    using Microsoft.Robotics.Interaction.Runtime.Input;
    using Microsoft.Robotics.Navigation.Motion;
    using Microsoft.Robotics.Navigation.Perception;
    using Microsoft.Robotics.Navigation.Runtime.Localization;
    using Microsoft.Robotics.Navigation.Runtime.Motion;
    using Microsoft.Robotics.Navigation.Runtime.Perception;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Tracing;

    /// <summary>
    /// Path tracking drive agent.
    /// </summary>
    [DataContract]
    public class PathTrackingDriveAgent :
        ConsumerProducerAgent<SimpleAgentMessage<DriveDirection>, MobileBaseVelocityReferenceMessage>, 
        ISubscriptionReceiver<SimpleAgentMessage<Pose2D>>,
        ISubscriptionReceiver<SimpleAgentMessage<DriveFeedbackState>>,
        ISubscriptionReceiver<SimpleAgentMessage<KinectHorizontalDepthProfile>>
    {
        /// <summary>
        /// Name of the agent producing pose information messages.
        /// </summary>
        [DataMember(Name = "PoseProducer")]
        private readonly string poseProducer;

        /// <summary>
        /// Name of agent producing horizontal depth profile messages.
        /// </summary>
        [DataMember(Name = "HdpProducer")]
        private readonly string hdpProducer;

        /// <summary>
        /// Name of agent producing drive feedback messages.
        /// </summary>
        [DataMember(Name = "DriveFeedbackProducer")]
        private readonly string driveFeedbackProducer;

        /// <summary>
        /// Number of milliseconds between drive commands before triggering full stop.
        /// </summary>
        [DataMember(Name = "DeadmanTriggerMilliseconds")]
        private readonly long deadmanTriggerMilliseconds;

        /// <summary>
        /// Threshold nearer than which force should be 1 for a flat wall.
        /// </summary>
        [DataMember(Name = "ObstacleAvoidanceDistanceThreshold")]
        private double obstacleAvoidanceDistanceThreshold;

        /// <summary>
        /// Gain applied to heading adjustment.
        /// </summary>
        [DataMember(Name = "ObstacleAvoidanceHeadingAdjustmentGain")]
        private double obstacleAvoidanceHeadingAdjustmentGain;

        /// <summary>
        /// Maximum heading adjustment.
        /// </summary>
        [DataMember(Name = "ObstacleAvoidanceMaximumHeadingAdjustment")]
        private double obstacleAvoidanceMaximumHeadingAdjustment;

        /// <summary>
        /// Corridor to left/right of robot within which we consider depth readings.
        /// </summary>
        [DataMember(Name = "CorridorRadius")]
        private double corridorRadius;

        /// <summary>
        /// Agent producing pose information messages.
        /// </summary>
        private IAgentHost poseProducerAgent;

        /// <summary>
        /// Agent producing horizontal depth profile messages.
        /// </summary>
        private IAgentHost hdpProducerAgent;

        /// <summary>
        /// Agent producing drive feedback messages.
        /// </summary>
        private IAgentHost driveFeedbackProducerAgent;

        /// <summary>
        /// Current drive direction.
        /// </summary>
        private DriveDirection currentDriveDirection;

        /// <summary>
        /// Timestamp of last received drive direction.
        /// </summary>
        private DateTime lastDriveDirectionTime = DateTime.MaxValue;

        /// <summary>
        /// Last velocity (used for smoothing).
        /// </summary>
        private MobileBaseVelocityReference velocity;

        /// <summary>
        /// Last received robot pose.
        /// </summary>
        private double previousHeading;

        /// <summary>
        /// Last received robot pose.
        /// </summary>
        private double currentHeading;

        /// <summary>
        /// Last received drive feedback state.
        /// </summary>
        private DriveFeedbackState currentDriveFeedback;

        /// <summary>
        /// Last seen HDP depth readings.
        /// </summary>
        private KinectHorizontalDepthProfile lastHdp;

        /// <summary>
        /// Drive state at the time of the last received DriveDirection.
        /// </summary>
        private DriveFeedbackState driveFeedbackAtLastDirection;

        /// <summary>
        /// Maximum turn rate in radians per second.
        /// </summary>
        [DataMember(Name = "MaxTurnRate")]
        private double maxTurnRate;

        /// <summary>
        /// Factor by which turn rate is scaled while backing.
        /// </summary>
        [DataMember(Name = "BackingTurnRateFactor")]
        private double backingTurnRateFactor;

        /// <summary>
        /// Maximum speed in meters per second.
        /// </summary>
        [DataMember(Name = "MaxSpeed")]
        private double maxSpeed;

        /// <summary>
        /// Speed factor proportional to turning speed (slow down/stop to turn).
        /// </summary>
        [DataMember(Name = "TurnGain")]
        private double turnGain;

        /// <summary>
        /// Dampening factor as heading converges.
        /// </summary>
        [DataMember(Name = "HeadingDampening")]
        private double headingDampening;

        /// <summary>
        /// Factor of max speed, by which robot is allowed to accelerate.
        /// </summary>
        [DataMember(Name = "ForwardAccelerationFactor")]
        private double forwardAccelerationFactor;

        /// <summary>
        /// Factor of max speed, by which robot is allowed to decelerate.
        /// </summary>
        [DataMember(Name = "ForwardDecelerationFactor")]
        private double forwardDecelerationFactor;

        /// <summary>
        /// Factor of max turn rate, by which robot is allowed to accelerate.
        /// </summary>
        [DataMember(Name = "TurnAccelerationFactor")]
        private double turnAccelerationFactor;

        /// <summary>
        /// Factor of max turn rate, by which robot is allowed to decelerate.
        /// </summary>
        [DataMember(Name = "TurnDecelerationFactor")]
        private double turnDecelerationFactor;

        /// <summary>
        /// Initializes a new instance of the PathTrackingDriveAgent class.
        /// </summary>
        /// <param name="name">Agent name</param>
        /// <param name="driveDirectionProducer">Name of agent producing drive direction messages.</param>
        /// <param name="poseProducer">Name of agent producing pose information messages.</param>
        /// <param name="hdpProducer">Name of agent producing horizontal depth profile messages.</param>
        /// <param name="driveFeedbackProducer">Name of agent producing drive feedback messages.</param>
        /// <param name="maxTurnRate">Maximum turn rate in radians per second.</param>
        /// <param name="backingTurnRateFactor">Factor by which turn rate is scaled while backing</param>
        /// <param name="maxSpeed">Maximum speed in meters per second.</param>
        /// <param name="turnGain">Speed factor proportional to turning speed (slow down/stop to turn).</param>
        /// <param name="headingDampening">Dampening factor as heading converges.</param>
        /// <param name="forwardAccelerationFactor">Factor of max speed, by which robot is allowed to accelerate.</param>
        /// <param name="forwardDecelerationFactor">Factor of max speed, by which robot is allowed to decelerate.</param>
        /// <param name="turnAccelerationFactor">Factor of max turn rate, by which robot is allowed to accelerate.</param>
        /// <param name="turnDecelerationFactor">Factor of max turn rate, by which robot is allowed to decelerate.</param>
        /// <param name="obstacleAvoidanceDistanceThreshold">Threshold nearer than which force should be 1 for a flat wall.</param>
        /// <param name="obstacleAvoidanceHeadingAdjustmentGain">Gain applied to heading adjustment.</param>
        /// <param name="obstacleAvoidanceMaximumHeadingAdjustment">Maximum heading adjustment.</param>
        /// <param name="corridorRadius">Corridor to left/right of robot within which we consider depth readings.</param>
        /// <param name="deadmanTriggerMilliseconds">Number of milliseconds between drive commands before triggering full stop.</param>
        public PathTrackingDriveAgent(
            string name,
            IProducer<SimpleAgentMessage<DriveDirection>> driveDirectionProducer,
            IProducer<SimpleAgentMessage<Pose2D>> poseProducer,
            IProducer<SimpleAgentMessage<KinectHorizontalDepthProfile>> hdpProducer,
            IProducer<SimpleAgentMessage<DriveFeedbackState>> driveFeedbackProducer,
            double maxTurnRate,
            double backingTurnRateFactor,
            double maxSpeed,
            double turnGain,
            double headingDampening,
            double forwardAccelerationFactor,
            double forwardDecelerationFactor,
            double turnAccelerationFactor,
            double turnDecelerationFactor,
            double obstacleAvoidanceDistanceThreshold,
            double obstacleAvoidanceHeadingAdjustmentGain,
            double obstacleAvoidanceMaximumHeadingAdjustment,
            double corridorRadius,
            long deadmanTriggerMilliseconds)
            : base(name, driveDirectionProducer.Name)
        {
            this.poseProducer = poseProducer.Name;
            this.hdpProducer = hdpProducer.Name;
            this.driveFeedbackProducer = driveFeedbackProducer.Name;
            this.maxTurnRate = maxTurnRate;
            this.backingTurnRateFactor = backingTurnRateFactor;
            this.maxSpeed = maxSpeed;
            this.turnGain = turnGain;
            this.headingDampening = headingDampening;
            this.forwardAccelerationFactor = forwardAccelerationFactor;
            this.forwardDecelerationFactor = forwardDecelerationFactor;
            this.turnAccelerationFactor = turnAccelerationFactor;
            this.turnDecelerationFactor = turnDecelerationFactor;
            this.obstacleAvoidanceDistanceThreshold = obstacleAvoidanceDistanceThreshold;
            this.obstacleAvoidanceHeadingAdjustmentGain = obstacleAvoidanceHeadingAdjustmentGain;
            this.obstacleAvoidanceMaximumHeadingAdjustment = obstacleAvoidanceMaximumHeadingAdjustment;
            this.corridorRadius = corridorRadius;
            this.deadmanTriggerMilliseconds = deadmanTriggerMilliseconds;
        }

        /// <summary>
        /// Called when agent is initialized
        /// </summary>
        /// <param name="locator">An agent locator</param>
        public override void Initialize(AgentLocator locator)
        {
            base.Initialize(locator);

            this.poseProducerAgent = locator.GetAgent(this.poseProducer);
            this.poseProducerAgent.Subscribe<SimpleAgentMessage<Pose2D>>(this);
            this.hdpProducerAgent = locator.GetAgent(this.hdpProducer);
            this.hdpProducerAgent.Subscribe<SimpleAgentMessage<KinectHorizontalDepthProfile>>(this);
            this.driveFeedbackProducerAgent = locator.GetAgent(this.driveFeedbackProducer);
            this.driveFeedbackProducerAgent.Subscribe<SimpleAgentMessage<DriveFeedbackState>>(this);

            this.currentDriveDirection = DriveDirection.Stop;
            this.velocity = new MobileBaseVelocityReference(0, 0, 0);
        }

        /// <summary>
        /// Called when the agent is activated.
        /// </summary>
        public override void OnActivated()
        {
            base.OnActivated();

            this.poseProducerAgent.Activate();
            this.hdpProducerAgent.Activate();

            this.driveFeedbackProducerAgent.Subscribe<SimpleAgentMessage<DriveFeedbackState>>(this);
            this.driveFeedbackProducerAgent.Activate();
        }

        /// <summary>
        /// Called when the agent is deactivated.
        /// </summary>
        public override void OnDeactivated()
        {
            base.OnDeactivated();
            
            this.poseProducerAgent.Deactivate();
            this.hdpProducerAgent.Deactivate();
            this.driveFeedbackProducerAgent.Deactivate();
        }

        /// <summary>
        /// Update our state based on drive direction.
        /// </summary>
        /// <param name="drive">A simple DriveDirection message</param>
        public override void Receive(SimpleAgentMessage<DriveDirection> drive)
        {
            TraceOut.Info(TraceContexts.Nav, "Track to: {0}, {1}, {2}", drive.State.Forward, drive.State.Distance, drive.State.Heading);
            this.currentDriveDirection = drive.State;
            this.lastDriveDirectionTime = DateTime.Now;
            this.driveFeedbackAtLastDirection = this.currentDriveFeedback;
        }

        /// <summary>
        /// Process pose information messages.
        /// </summary>
        /// <param name="pose">Pose message.</param>
        public void Receive(SimpleAgentMessage<Pose2D> pose)
        {
            this.previousHeading = this.currentHeading;
            this.currentHeading = PathPlanningCommon.FixHeadingRange(pose.State.Heading);
        }

        /// <summary>
        /// Process horizontal depth profile messages.
        /// </summary>
        /// <param name="message">Horizontal depth profile message.</param>
        public void Receive(SimpleAgentMessage<KinectHorizontalDepthProfile> message)
        {
            this.lastHdp = message.State;
        }

        /// <summary>
        /// Process drive feedback messages.
        /// </summary>
        /// <param name="message">Drive feedback message.</param>
        public void Receive(SimpleAgentMessage<DriveFeedbackState> message)
        {
            this.currentDriveFeedback = message.State;

            if (this.currentDriveFeedback != null && this.driveFeedbackAtLastDirection != null)
            {
                if ((DateTime.Now - this.lastDriveDirectionTime).TotalMilliseconds <= this.deadmanTriggerMilliseconds)
                {
                    Pose2D deltaPosition = this.currentDriveFeedback.GetMeanDeltaPose(this.driveFeedbackAtLastDirection);
                    
                    double speedFactor = PathPlanningCommon.SpeedFactorApproachingTarget(
                        this.currentDriveDirection,
                        deltaPosition.TotalTranslation());

                    double backingFactor = this.currentDriveDirection.Forward < 0 ? this.backingTurnRateFactor : 1;

                    this.velocity = PathPlanningCommon.Drive(
                        this.currentDriveDirection,
                        //// The "potential field repulsion drive" is disables for now.
                        ////SuggestionDrive.AdjustDrive(
                        ////    PathPlanningCommon.LimitForwardSpeed(this.currentDriveDirection, speedFactor),
                        ////    this.lastHdp,
                        ////    this.obstacleAvoidanceDistanceThreshold,
                        ////    this.obstacleAvoidanceHeadingAdjustmentGain,
                        ////    this.obstacleAvoidanceMaximumHeadingAdjustment,
                        ////    this.corridorRadius),
                        this.previousHeading,
                        this.currentHeading,
                        this.velocity,
                        this.maxTurnRate * speedFactor * backingFactor,
                        this.maxSpeed,
                        this.turnGain,
                        this.headingDampening,
                        this.forwardAccelerationFactor,
                        this.forwardDecelerationFactor,
                        this.turnAccelerationFactor,
                        this.turnDecelerationFactor);

                    this.Publisher.Post(new MobileBaseVelocityReferenceMessage(this.velocity, message));
                    TraceOut.Info(TraceContexts.Nav, "MobileBaseVelocity: {0}, {1}, {2}", this.velocity.XInMetersPerSecond, this.velocity.YInMetersPerSecond, this.velocity.ThetaInRadiansPerSecond);
                }
                else
                {
                    // stop due to extended time since last drive command
                    double ms = (DateTime.Now - this.lastDriveDirectionTime).TotalMilliseconds;
                    Console.WriteLine("Deadman Stop ({0}ms).", ms);
                    TraceOut.Warning(TraceContexts.Nav, "Deadman Stop ({0}ms).", ms);
                    this.Publisher.Post(new MobileBaseVelocityReferenceMessage(new MobileBaseVelocityReference(0, 0, 0), message));
                }
            }
        }
    }
}
